//
// Created by lijiayi on 19-8-5.
//
#include "ros/ros.h"
#include "serial/serial.h"
#include <string>
#include <sstream>
#include <iostream>
#include <fstream>
#include <vector>
#include <ctime>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/AccelStamped.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <ros/package.h>


#define  BYTE0(dwTemp)       ( *( (uint8_t *)(&dwTemp)    )  )
#define  BYTE1(dwTemp)       ( *( (uint8_t *)(&dwTemp) + 1) )
#define  BYTE2(dwTemp)       ( *( (uint8_t *)(&dwTemp) + 2) )
#define  BYTE3(dwTemp)       ( *( (uint8_t *)(&dwTemp) + 3) )

typedef enum {
    UP_VERSION = 0x00,
    UP_STATUS = 0x01,
    UP_SENSER = 0x02,
    UP_RCDATA = 0x03,
    UP_GPSDATA = 0x04,
    UP_POWER = 0x05,
    UP_MOTOR = 0x06,
    UP_SENSER2 = 0x07,
    UP_FLYMODE = 0x0A,
    UP_SPEED = 0x0B,
    UP_PID1 = 0x10,
    UP_PID2 = 0x11,
    UP_PID3 = 0x12,
    UP_PID4 = 0x13,
    UP_PID5 = 0x14,
    UP_PID6 = 0x15,
    UP_RADIO = 0x40,
    UP_MSG = 0xEE,
    UP_CHECK = 0xEF,

    UP_REMOTER = 0x50,
    UP_PRINTF = 0x51,

    UP_USER_DATA1 = 0xF1,
    UP_USER_DATA2 = 0xF2,
    UP_USER_DATA3 = 0xF3,
    UP_USER_DATA4 = 0xF4,
    UP_USER_DATA5 = 0xF5,
    UP_USER_DATA6 = 0xF6,
    UP_USER_DATA7 = 0xF7,
    UP_USER_DATA8 = 0xF8,
    UP_USER_DATA9 = 0xF9,
    UP_USER_DATA10 = 0xFA,
} upmsgID_e;


#define  D_COMMAND_ACC_CALIB        0x01
#define  D_COMMAND_GYRO_CALIB        0x02
#define  D_COMMAND_MAG_CALIB        0x04
#define  D_COMMAND_BARO_CALIB        0x05
#define  D_COMMAND_ACC_CALIB_EXIT    0x20
#define  D_COMMAND_ACC_CALIB_STEP1    0x21
#define  D_COMMAND_ACC_CALIB_STEP2    0x22
#define  D_COMMAND_ACC_CALIB_STEP3    0x23
#define  D_COMMAND_ACC_CALIB_STEP4    0x24
#define  D_COMMAND_ACC_CALIB_STEP5    0x25
#define  D_COMMAND_ACC_CALIB_STEP6    0x26
#define  D_COMMAND_FLIGHT_LOCK        0xA0
#define  D_COMMAND_FLIGHT_ULOCK        0xA1

#define  D_ACK_READ_PID                0x01
#define  D_ACK_READ_VERSION            0xA0
#define  D_ACK_RESET_PARAM            0xA1

typedef enum {
    DOWN_COMMAND = 0x01,
    DOWN_ACK = 0x02,
    DOWN_RCDATA = 0x03,
    DOWN_POWER = 0x05,
    DOWN_FLYMODE = 0x0A,
    DOWN_PID1 = 0x10,//rpy angle rate
    DOWN_PID2 = 0x11,//rpy angle
    DOWN_PID3 = 0x12,//vx vy vz
    DOWN_PID4 = 0x13,//x y z
    DOWN_PID5 = 0x14,
    DOWN_PID6 = 0x15,
    DOWN_RADIO = 0x40,

    DOWN_REMOTER = 0x50,
    DOWN_POSITION = 0x60, //add by ljy
} downmsgID_e;
#define  POS_NEXT 1
#define  POS_NOW 0


using namespace std;

typedef struct {
    float roll;
    float pitch;
    float yaw;
    float thrust;
    float trimPitch;
    float trimRoll;
    uint8_t ctrlMode;
    bool flightMode;
    bool RCLock;
} remoterData_t;

typedef struct {
    float x;
    float y;
    float z;
} pos_t;

typedef struct {
    pos_t currentPos;
    pos_t desiredPos;
} pos_now_next_t;

typedef struct {
    int16_t kp;
    int16_t ki;
    int16_t kd;
} PID_t;


int main(int argc, char **argv) {
    ros::init(argc, argv, "receive_serial_node");
    ros::NodeHandle nodeHandle;
    serial::Serial sr;
    std::string dev_name = "/dev/ttyACM0";
    int baudrate = 500000;


    try {
        sr.setPort(dev_name);
        sr.setBaudrate(baudrate);
        serial::Timeout to = serial::Timeout::simpleTimeout(10000);
        sr.setTimeout(to);
        sr.open();
    }
    catch (serial::IOException &e) {
        ROS_ERROR_STREAM("Unable to open port ");
    }
    if (sr.isOpen()) {
        ROS_INFO_STREAM("Serial Port opened");
    } else {
        ROS_ERROR_STREAM("No port opened");
    }

    ofstream p_v_a_psp_vsp_prysp;
    std::string pkg_path = ros::package::getPath("drone_pnp");
    p_v_a_psp_vsp_prysp.open(pkg_path + "/data/p_v_a_psp_vsp_prysp.csv");
    if (!p_v_a_psp_vsp_prysp.is_open()) {
        ROS_ERROR("%s",("Can't write data file to:  " + pkg_path + "/data/").c_str());
        return 0;
    }


    ros::Publisher pos_pub = nodeHandle.advertise<geometry_msgs::PoseStamped>("/fused_pos/pose", 1);
    ros::Publisher vel_pub = nodeHandle.advertise<geometry_msgs::TwistStamped>("/fused_pos/vel", 1);
    ros::Publisher acc_pub = nodeHandle.advertise<geometry_msgs::AccelStamped>("/fused_pos/acc", 1);
    geometry_msgs::PoseStamped pos;
    geometry_msgs::TwistStamped vel;
    geometry_msgs::AccelStamped acc;
    ros::Publisher data_pub1 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/a_v_vnow_pnow__thru_thruBase", 1);
    geometry_msgs::PoseStamped data1;

    ros::Time start_Time = ros::Time::now();
    int last_record_endl = 3;

    while (ros::ok()) {
        if (sr.available()) {
            if ((uint8_t) sr.read(1)[0] == 0xAA)
                if ((uint8_t) sr.read(1)[0] == 0xAA) {
                    uint8_t fun_byte = (uint8_t) sr.read(1)[0];
                    if (fun_byte == UP_USER_DATA1) {

                        uint8_t len, sum = 0;
                        len = (uint8_t) sr.read(1)[0];
                        if (len <= 30) {
                            vector<uint8_t> buffer;
                            sr.read(buffer, len + 1);
                            sum += 0xAA + 0xAA + UP_USER_DATA1 + len;
                            for (int i = 0; i < len; i++) {
                                sum += buffer[i];
                            }
                            if (sum == buffer[len]) {
                                acc.header.stamp = vel.header.stamp = pos.header.stamp = ros::Time::now();
                                acc.accel.linear.x = 0.01 * 0.01 * (int16_t) (buffer[0] << 8 | buffer[1]);
                                acc.accel.linear.y = 0.01 * 0.01 * (int16_t) (buffer[2] << 8 | buffer[3]);
                                acc.accel.linear.z = 0.01 * 0.01 * (int16_t) (buffer[4] << 8 | buffer[5]);
                                vel.twist.linear.x = 0.01 * 0.01 * (int16_t) (buffer[6] << 8 | buffer[7]);
                                vel.twist.linear.y = 0.01 * 0.01 * (int16_t) (buffer[8] << 8 | buffer[9]);
                                vel.twist.linear.z = 0.01 * 0.01 * (int16_t) (buffer[10] << 8 | buffer[11]);
                                pos.pose.position.x = 0.01 * 0.01 * (int16_t) (buffer[12] << 8 | buffer[13]);
                                pos.pose.position.y = 0.01 * 0.01 * (int16_t) (buffer[14] << 8 | buffer[15]);
                                pos.pose.position.z = 0.01 * 0.01 * (int16_t) (buffer[16] << 8 | buffer[17]);
                                pos_pub.publish(pos);
                                vel_pub.publish(vel);
                                acc_pub.publish(acc);
//                                cout << "Pos:" << endl << pos.pose.position << endl;
//                                cout << "vel:" << endl << vel.twist.linear << endl;
//                                cout << "acc:" << endl << acc.accel.linear << endl;
                                if(last_record_endl==3)
                                {
                                    p_v_a_psp_vsp_prysp << (ros::Time::now()-start_Time).toSec()
                                                  << "," << pos.pose.position.x << "," << pos.pose.position.y << "," << pos.pose.position.z
                                                  << "," << vel.twist.linear.x << "," << vel.twist.linear.y << "," << vel.twist.linear.z
                                                  << "," << acc.accel.linear.x << "," << acc.accel.linear.y << "," << acc.accel.linear.z << "," ;
                                    last_record_endl = 1;
                                }
                            }
                        }
                    } else if (fun_byte == UP_USER_DATA2) {
                        uint8_t len, sum = 0;
                        len = (uint8_t) sr.read(1)[0];
                        if (len <= 30) {
                            vector<uint8_t> buffer;
                            sr.read(buffer, len + 1);
                            sum += 0xAA + 0xAA + UP_USER_DATA2 + len;
                            for (int i = 0; i < len; i++) {
                                sum += buffer[i];
                            }
                            if (sum == buffer[len]) {
                                geometry_msgs::PoseStamped pos_temp;
                                geometry_msgs::TwistStamped vel_temp;
                                geometry_msgs::AccelStamped acc_temp;
                                acc_temp.header.stamp = vel_temp.header.stamp = pos_temp.header.stamp = ros::Time::now();
                                pos_temp.pose.orientation.x = 0.01 * (int16_t) (buffer[0] << 8 | buffer[1]);
                                pos_temp.pose.orientation.y = 0.01 * (int16_t) (buffer[2] << 8 | buffer[3]);
                                pos_temp.pose.orientation.z = 0.01 * (int16_t) (buffer[4] << 8 | buffer[5]);
                                vel_temp.twist.linear.x = 0.01 * 0.01 * (int16_t) (buffer[6] << 8 | buffer[7]);
                                vel_temp.twist.linear.y = 0.01 * 0.01 * (int16_t) (buffer[8] << 8 | buffer[9]);
                                vel_temp.twist.linear.z = 0.01 * 0.01 * (int16_t) (buffer[10] << 8 | buffer[11]);
                                pos_temp.pose.position.x = 0.01 * 0.01 * (int16_t) (buffer[12] << 8 | buffer[13]);
                                pos_temp.pose.position.y = 0.01 * 0.01 * (int16_t) (buffer[14] << 8 | buffer[15]);
                                pos_temp.pose.position.z = 0.01 * 0.01 * (int16_t) (buffer[16] << 8 | buffer[17]);

                                if(last_record_endl==1)
                                {
                                    p_v_a_psp_vsp_prysp << (ros::Time::now()-start_Time).toSec()
                                        << "," << pos_temp.pose.position.x << "," << pos_temp.pose.position.y << "," << pos_temp.pose.position.z
                                        << "," << vel_temp.twist.linear.x << "," << vel_temp.twist.linear.y << "," << vel_temp.twist.linear.z
                                        << "," << pos_temp.pose.orientation.x << "," << pos_temp.pose.orientation.y << "," << pos_temp.pose.orientation.z
                                        << ",";
                                    ROS_INFO_STREAM(pos_temp.pose.orientation.x << "," << pos_temp.pose.orientation.y << "," << pos_temp.pose.orientation.z << "~~~~~!!!~!~!~!~!~") ;
                                    last_record_endl = 2;
                                }



//                                data1.header.stamp = ros::Time::now();
//                                data1.pose.position.x = 0.01 * 0.01 * (int16_t) (buffer[0] << 8 | buffer[1]);
//                                data1.pose.position.y = 0.01 * 0.01 * (int16_t) (buffer[2] << 8 | buffer[3]);
//                                data1.pose.position.z = 0.01 * 0.01 * (int16_t) (buffer[4] << 8 | buffer[5]);
//                                data1.pose.orientation.x = 0.01 * 0.01 * (int16_t) (buffer[6] << 8 | buffer[7]);
////                                data1.pose.orientation.y = 0.01 * 0.01 * (int16_t) (buffer[8] << 8 | buffer[9]);
////                                data1.pose.orientation.z = 0.01 * 0.01 * (int16_t) (buffer[10] << 8 | buffer[11]);
////                                data1.pose.position.x = 0.01 * 0.01 * (int16_t) (buffer[12] << 8 | buffer[13]);
//                                data1.pose.orientation.y =  (int16_t) (buffer[14] << 8 | buffer[15]);
//                                data1.pose.orientation.z =   (int16_t) (buffer[16] << 8 | buffer[17]);
//                                data_pub1.publish(data1);
////                                vel_pub.publish(vel);
////                                acc_pub.publish(acc);
//                                cout << "acc_x:" << endl << data1.pose.position.x << endl;
//                                cout << "v_x:" << endl << data1.pose.position.y << endl;
//                                cout << "vnow_x:" << endl << data1.pose.position.z << endl;
//                                cout << "pnow_x:" << endl << data1.pose.orientation.x << endl;
//                                cout << "thru:" << endl << data1.pose.orientation.y << endl;
//                                cout << "thrubase:" << endl << data1.pose.orientation.z << endl;
////                                cout << "vel:" << endl << vel.twist.linear << endl;
////                                cout << "acc:" << endl << acc.accel.linear << endl;
                            }
                        }
                    }else if (fun_byte == UP_USER_DATA3)
                    {
                        uint8_t len, sum = 0;
                        len = (uint8_t) sr.read(1)[0];
                        if (len <= 30)
                        {
                            vector<uint8_t> buffer;
                            sr.read(buffer, len + 1);
                            sum += 0xAA + 0xAA + UP_USER_DATA3 + len;
                            for (int i = 0; i < len; i++)
                            {
                                sum += buffer[i];
                            }
                            if (sum == buffer[len])
                            {
                                geometry_msgs::PoseStamped pos_temp;
                                geometry_msgs::TwistStamped vel_temp;
                                geometry_msgs::AccelStamped acc_temp;
                                acc_temp.header.stamp = vel_temp.header.stamp = pos_temp.header.stamp = ros::Time::now();
                                pos_temp.pose.orientation.x = -0.01 * (int16_t) (buffer[0] << 8 | buffer[1]);
                                pos_temp.pose.orientation.y = 0.01 * (int16_t) (buffer[2] << 8 | buffer[3]);
                                pos_temp.pose.orientation.z = 0.01 * (int16_t) (buffer[4] << 8 | buffer[5]);
                                vel_temp.twist.linear.x = 0.01 * 0.01 * (int16_t) (buffer[6] << 8 | buffer[7]);
                                vel_temp.twist.linear.y = 0.01 * 0.01 * (int16_t) (buffer[8] << 8 | buffer[9]);
                                vel_temp.twist.linear.z = 0.01 * 0.01 * (int16_t) (buffer[10] << 8 | buffer[11]);
                                pos_temp.pose.position.x = 0.01 * 0.01 * (int16_t) (buffer[12] << 8 | buffer[13]);
                                pos_temp.pose.position.y = 0.01 * 0.01 * (int16_t) (buffer[14] << 8 | buffer[15]);
                                pos_temp.pose.position.z = 0.01 * 0.01 * (int16_t) (buffer[16] << 8 | buffer[17]);

                                if (last_record_endl == 2)
                                {
                                    p_v_a_psp_vsp_prysp << (ros::Time::now() - start_Time).toSec() << ","
                                                        << pos_temp.pose.orientation.x << ","
                                                        << pos_temp.pose.orientation.y << ","
                                                        << pos_temp.pose.orientation.z << ","
                                                        << vel_temp.twist.linear.x << ","
                                                        << vel_temp.twist.linear.y << ","
                                                        << vel_temp.twist.linear.z << ","
                                                        << endl;
                                    last_record_endl = 3;
                                }



//                                data1.header.stamp = ros::Time::now();
//                                data1.pose.position.x = 0.01 * 0.01 * (int16_t) (buffer[0] << 8 | buffer[1]);
//                                data1.pose.position.y = 0.01 * 0.01 * (int16_t) (buffer[2] << 8 | buffer[3]);
//                                data1.pose.position.z = 0.01 * 0.01 * (int16_t) (buffer[4] << 8 | buffer[5]);
//                                data1.pose.orientation.x = 0.01 * 0.01 * (int16_t) (buffer[6] << 8 | buffer[7]);
////                                data1.pose.orientation.y = 0.01 * 0.01 * (int16_t) (buffer[8] << 8 | buffer[9]);
////                                data1.pose.orientation.z = 0.01 * 0.01 * (int16_t) (buffer[10] << 8 | buffer[11]);
////                                data1.pose.position.x = 0.01 * 0.01 * (int16_t) (buffer[12] << 8 | buffer[13]);
//                                data1.pose.orientation.y =  (int16_t) (buffer[14] << 8 | buffer[15]);
//                                data1.pose.orientation.z =   (int16_t) (buffer[16] << 8 | buffer[17]);
//                                data_pub1.publish(data1);
////                                vel_pub.publish(vel);
////                                acc_pub.publish(acc);
//                                cout << "acc_x:" << endl << data1.pose.position.x << endl;
//                                cout << "v_x:" << endl << data1.pose.position.y << endl;
//                                cout << "vnow_x:" << endl << data1.pose.position.z << endl;
//                                cout << "pnow_x:" << endl << data1.pose.orientation.x << endl;
//                                cout << "thru:" << endl << data1.pose.orientation.y << endl;
//                                cout << "thrubase:" << endl << data1.pose.orientation.z << endl;
////                                cout << "vel:" << endl << vel.twist.linear << endl;
////                                cout << "acc:" << endl << acc.accel.linear << endl;
                            }
                        }
                    }
                }

        }
    }

    return 0;
}

